function plot_graph_slam_result(true_traj, opt_poses, landmarks, opt_landmarks)
% Graph-SLAM最终结果可视化

    clf;
    
    % 子图1: 轨迹对比
    subplot(1, 2, 1); hold on; grid on; axis equal;
    plot(landmarks(:, 1), landmarks(:, 2), 'k*', 'MarkerSize', 12, 'LineWidth', 2);
    plot(true_traj(1, :), true_traj(2, :), 'g-', 'LineWidth', 2);
    plot(opt_poses(1, :), opt_poses(2, :), 'b--', 'LineWidth', 2);
    
    if opt_landmarks.Count > 0
        lm_keys = cell2mat(keys(opt_landmarks));
        est_lm = zeros(length(lm_keys), 2);
        for i = 1:length(lm_keys)
            lm_pos = opt_landmarks(lm_keys(i));
            est_lm(i, :) = lm_pos';
        end
        plot(est_lm(:, 1), est_lm(:, 2), 'ro', 'MarkerSize', 8, 'LineWidth', 2);
    end
    
    legend('真实路标', '真实轨迹', '优化轨迹', '估计路标', 'Location', 'best');
    xlabel('X(m)'); ylabel('Y(m)');
    title('Graph-SLAM 轨迹对比');
    xlim([-8, 50]); ylim([-8, 50]);
    
    % 子图2: 误差分析
    subplot(1, 2, 2); hold on; grid on;
    position_error = sqrt(sum((true_traj(1:2, :) - opt_poses(1:2, :)).^2, 1));
    plot(position_error, 'b-', 'LineWidth', 1.5);
    xlabel('时间步'); ylabel('位置误差(m)');
    title(sprintf('位置误差 (均值=%.3fm)', mean(position_error)));
end

